/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package org.usfirst.team3132.demo;

import edu.wpi.first.wpilibj.*;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the SimpleRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class Robot5152 extends SimpleRobot {
    
    final int kP = 0;
    final int kI = 0;
    final int kD = 0;
    
    float motorSpeed = 0;
    float motorRotation = 0;
    
    //Encoder int values
    int leftEncoderRPM = 0;
    int rightEncoderRPM = 0;
    
    double gyroAngle = 0;
    double accelerometerValue = 0;
    
    // RobotDrive assumes that it's using Jaguars (we are).
    //RobotDrive drive = new RobotDrive(leftMotorChannel, rightMotorChannel);
    
    //Use Jaguars now because off PIDController
    Jaguar leftMotor = new Jaguar(1);
    Jaguar rightMotor = new Jaguar(2);
    Joystick leftStick = new Joystick(1);
    Joystick rightStick = new Joystick(2);
    Encoder leftEncoder = new Encoder(1,2);
    Encoder rightEncoder = new Encoder(3,4);
    PIDController leftPidController;
    PIDController rightPidController;
    
    //Gyro gyro = new Gyro(1);
    //Accelerometer accelerometer = new Accelerometer(1,4);
    //DriverStationLCD driverStationLCD = DriverStationLCD.getInstance();
    
    public Robot5152() {
        System.out.println("It's alive!");
        // Disable watchdog.
        //drive.setSafetyEnabled(false);
        leftMotor.setSafetyEnabled(false);
        rightMotor.setSafetyEnabled(false);
        leftPidController = new PIDController(kP,kI,kD,leftEncoder,leftMotor);
        rightPidController = new PIDController(kP,kI,kD,rightEncoder,rightMotor);
    }
    /**
     * This function is called once each time the robot enters autonomous mode.
     */
    public void autonomous() {
        /*System.out.println("autonomus mode");
        for (int i = 0; i < 4; i++) {
            drive.drive(0.5, 0.0);  // drive 50% forward speed with 0% turn
            Timer.delay(2.0);  // wait 2 seconds
            drive.drive(0.0, 0.75); // drive 0% forward with 75% turn
            Timer.delay(0.75); // wait for the 90 degree turn to complete
        }
        drive.drive(0.0, 0.0);  // drive 0% forward with 0% turn (stop)*/
    }

    /**
     * This function is called once each time the robot enters operator control.
     */
    public void operatorControl() {
        System.out.println("autonomus mode");
        leftEncoder.start();
        rightEncoder.start();
        //gyro.reset();
        
        while (isOperatorControl() && isEnabled()) // loop during enabled teleop mode
        {   
            //drive.tankDrive(leftStick, rightStick);
            
            //leftEncoderRPM = (int)leftEncoder.getRate();
            //rightEncoderRPM = (int)rightEncoder.getRate();
            //gyroAngle = gyro.getAngle();
            //accelerometerValue = accelerometer.getAcceleration();
            //leftPidController.
            
            motorSpeed = (float)leftStick.getY();
            leftMotor.set(motorSpeed);
            rightMotor.set(motorSpeed);
                 
                    
            Timer.delay(0.5);
        }
        
        leftEncoder.stop();
        leftEncoder.reset();
        rightEncoder.stop();
        rightEncoder.reset();
    }
}
